last edits:
TODO:
Requires IPython to run in "pylab" mode. If this is not the case, at least insert
from pylab import *
or
%pylab inline
somewhere
This notebook continues scientific investigations from the original "AnkleSLIP" notebook.
It has been splitted to regain the general overview.
This script tries to identify a minimalistic SLIP extension (e.g., Ankle-Y-position only).
Requirements:
Step 0: configure notebook
Step 1: import data
Step 2: test consistency test consistency of SLIP parameters with kin. CoM data
Step 3: find minimal predictors
Step 3.1: retest consistency of SLIP parameters with kin. CoM
Step 3.2: compute predictors
Step 3.3: Select factors and reduced model
Step 3.4: test SLIP parameter prediction
Step 3.5: test self-predictability (consistency) of reduced model
Step 3.6: Compute (only) eigenvalues of full and reduced Floquet models
Step 3.7: Test different explicit models
Step 4: create controlled SLIP
step 4.1: find periodic solution
step 4.2: compute parameter prediction maps
step 4.3: define stride function
step 4.4: compute jacobian
Step 5: compute eigenvalues of different models
Step 6: visualize predictors
Step 7: test autonomy of [IC; factor1; factor2] system
Step 8: create autonomous model from factors
Step 9: Compare predictions of trajectories for SLIP vs. Floquet models NOTE: ongoing work (i.e. esp. ugly code)
Step 9.1: Select and prepare data
Step 9.2: create bootstrapped Floquet models
Step 9.3: test continous prediction for Floquet models
Step 9.4: (now obsolete)
Step 9.5: compute Phase for SLIP
Step 9.6: perform predictions
In [23]:
# define and import data
subject = 2 #available subjects: 1,2,3,7. Other subjects have comparatively bad data quality
ttype = 1 # 1: free running, 2: metronome running (data incomplete!)
n_factors = 3 # 1-5 factors to predict SLIP parameters
exclude_IC_from_factors = False # exclude the initial conditions from kinematic state
#prior to identifying SLIP parameter predictors?
# select ankle-SLIP instead of "factors without CoM state" ?
select_ankle_SLIP = True
# to compute periodic SLIP orbit: average over parameters or initial conditions?
po_average_over_IC = True
In [24]:
import mutils.io as mio
reload(mio)
import mutils.misc as mi
import os
import sys
import re
# load kinematic data
k = mio.KinData()
k.load(subject, ttype)
k.selection = ['r_anl_y - com_y', 'l_anl_y - com_y', 'com_z']
SlipData = [mi.Struct(mio.mload('../data/2013-newSlip/SLIP_s%it%ir%i.dict' % (subject, ttype, rep)))
for rep in k.reps]
indices_right = [mi.upper_phases(d.phases[:-1], sep=0, return_indices=True) for d in SlipData]
indices_left = [mi.upper_phases(d.phases[:-1], sep=pi, return_indices=True) for d in SlipData]
starts_right = [idxr[0] < idxl[0] for idxr, idxl in zip(indices_right, indices_left)]
param_right = [ vstack(d.P)[idxr, :] for d, idxr in zip(SlipData, indices_right)]
param_left = [ vstack(d.P)[idxl, :] for d, idxl in zip(SlipData, indices_left)]
IC_right = [ vstack(d.IC)[idxr, :] for d, idxr in zip(SlipData, indices_right)]
IC_left = [ vstack(d.IC)[idxl, :] for d, idxl in zip(SlipData, indices_left)]
In [25]:
# additional step parameters: step time, minimal apex height
TR = [vstack(d.T)[idxr, :] for d, idxr in zip(SlipData, indices_right)]
TL = [vstack(d.T)[idxl, :] for d, idxl in zip(SlipData, indices_left)]
yminR = [vstack(d.ymin)[idxr, :] for d, idxr in zip(SlipData, indices_right)]
yminL = [vstack(d.ymin)[idxl, :] for d, idxl in zip(SlipData, indices_left)]
In [26]:
#select any data
import models.sliputil as su
dat = SlipData[2]
stepnr = 32
print "simulation:", su.finalState(dat.IC[stepnr], dat.P[stepnr], {'m' : dat.mass, 'g': dat.g})
print '==================='
print "experiment:", dat.IC[stepnr + 1]
Step 3.1: Re-Test consistency of SLIP params with kin. CoM
Step 3.2: compute predictors
Step 3.3: Select factors and reduced model
Step 3.4: test SLIP parameter prediction
Step 3.5: test self-predictability (consistency) of reduced model
Step 3.6: Compute (only) eigenvalues of full and reduced Floquet models
Step 3.7: Test different explicit models
In [27]:
reload(mio)
# New: Each section gets its own data container.
# find minimal predictors
import mutils.statistics as st
import mutils.FDatAn as fda
# set apex data
# *NOTE* the first three labels have to be "com_x", "com_y", "com_z". If you edit this, make sure to edit the cell where data
# are further processed...
k.selection = [ 'com_x', 'com_y', 'com_z',
'r_anl_y - com_y', 'r_anl_x - com_x', 'r_mtv_z - r_anl_z', 'r_mtv_x - r_anl_x', 'r_kne_y - com_y',
'r_elb_y - com_y', 'r_elb_x - com_x', 'r_wrl_z - com_z', 'r_wrl_x - com_x', 'cvii_y - com_y',
'l_anl_y - com_y', 'l_anl_x - com_x', 'l_mtv_z - l_anl_z', 'l_mtv_x - l_anl_x', 'l_kne_y - com_y',
'l_elb_y - com_y', 'l_elb_x - com_x', 'l_wrl_z - com_z', 'l_wrl_x - com_x', ]
# sep = 0 -> right leg will touchdown next
# sep = pi -> right leg will touchdown next
# always exclude last apex - there are no SLIP parameters defined for it!
kin_right = k.get_kin_apex( [mi.upper_phases(d.phases[:-1], sep=0) for d in SlipData],)
kin_left = k.get_kin_apex( [mi.upper_phases(d.phases[:-1], sep=pi) for d in SlipData],)
In [69]:
# build common dataset
import mutils.io as mio
from mutils.io import build_dataset
reload(mio)
import mutils.misc as mi
import mutils.FDatAn as fda
In [71]:
In [72]:
# load kinematic data
if False: # take previously defined data?
subject = 3
ttype = 1
k = mio.KinData()
k.load(subject, ttype)
k.selection = ['com_x', 'com_y', 'com_z', 'r_anl_y - com_y', 'l_anl_y - com_y', 'com_z']
SlipData = [mi.Struct(mio.mload('../data/2013-newSlip/SLIP_s%it%ir%i.dict' % (subject, ttype, rep)))
for rep in k.reps]
d3orig = build_dataset(k, SlipData) # the d3 "original" workspace
all_IC_r = d3orig.all_IC_r
all_IC_l = d3orig.all_IC_l
all_param_r = d3orig.all_param_r
all_param_l = d3orig.all_param_l
all_kin_r = d3orig.all_kin_r
all_kin_l = d3orig.all_kin_l
all_phases_r = d3orig.all_phases_r
all_phases_l = d3orig.all_phases_l
param_right = d3orig.param_right
param_left = d3orig.param_left
IC_right = d3orig.IC_right
IC_left = d3orig.IC_left
TR = d3orig.TR
TL = d3orig.TL
yminR = d3orig.yminR
yminL = d3orig.yminL
kin_labels = d3orig.kin_labels
# copy to section 3 data container
d3 = mio.saveable() # this is a general container class that supplies a "save" and "load" functionality.
d3.kin_labels = kin_labels
d3.IC_r = all_IC_r
d3.IC_l = all_IC_l
d3.param_r = all_param_r
d3.param_l = all_param_l
d3.kin_r = all_kin_r
d3.kin_l = all_kin_l
d3.mass = mean([d.mass for d in SlipData])
d3orig.mass = mean([d.mass for d in SlipData])
step 3: find minimal predictors
to content
NOTE Here, the mass is not exactly the same as in the parameter calculations. Thus, minor deviations may occur.
In [30]:
stepnr = 1299
mass = mean([d.mass for d in SlipData])
# su.finalState actually performs a one-step integration of SLIP
print "simres [right step]:", su.finalState(d3orig.all_IC_r[stepnr, :], d3orig.all_param_r[stepnr, :], {'m' : d3orig.mass, 'g' : -9.81})
print "subsequent left apex:", d3orig.all_IC_l[stepnr, :]
print "==========="
print "simres [left step]:", su.finalState(d3orig.all_IC_l[stepnr, :], d3orig.all_param_l[stepnr, :], {'m' : d3orig.mass, 'g' : -9.81})
print "subsequent right apex:", d3orig.all_IC_r[stepnr + 1, :]
In [73]:
# detrend - look at residuals!
dt_kin_r = fda.dt_movingavg(all_kin_r, 30)
dt_kin_l = fda.dt_movingavg(all_kin_l, 30)
dt_param_r = fda.dt_movingavg(all_param_r, 30)
dt_param_l = fda.dt_movingavg(all_param_l, 30)
dt_IC_r = fda.dt_movingavg(all_IC_r, 30)
dt_IC_l = fda.dt_movingavg(all_IC_l, 30)
# use the *same* values for normalization for left and right!
# yes, it's 'sdt', not 'std': "Scores of DeTrented" ...
sdt_kin_r = dt_kin_r / std(dt_kin_r, axis=0)
sdt_kin_l = dt_kin_l / std(dt_kin_r, axis=0)
sdt_param_r = dt_param_r / std(dt_param_r, axis=0)
sdt_param_l = dt_param_l / std(dt_param_r, axis=0)
sdt_kin_r -= mean(sdt_kin_r, axis=0)
sdt_kin_l -= mean(sdt_kin_l, axis=0)
sdt_param_r -= mean(sdt_param_r, axis=0)
sdt_param_l -= mean(sdt_param_l, axis=0)
sdt_kin_r_noIC = hstack([sdt_kin_r[:,1:20],sdt_kin_r[:, 22:]])
sdt_kin_l_noIC = hstack([sdt_kin_l[:,1:20],sdt_kin_l[:, 22:]])
# append data to section 3 workspace
d3.s_kin_r = sdt_kin_r
d3.s_kin_l = sdt_kin_l
d3.s_param_r = sdt_param_r
d3.s_param_l = sdt_param_l
d3.s_kin_r_noIC = sdt_kin_r_noIC
d3.s_kin_l_noIC = sdt_kin_l_noIC
d3.dt_kin_r = dt_kin_r
d3.dt_kin_l = dt_kin_l
d3.dt_param_r = dt_param_r
d3.dt_param_l = dt_param_l
d3.dt_IC_r = dt_IC_r
d3.dt_IC_l = dt_IC_l
In [83]:
print dt_kin_r.shape, dt_param_r.shape
print dt_kin_l.shape, dt_param_l.shape
In [84]:
## TESTING - DELETE ME!
#sdt_kin_r.shape
s_IC_r = fda.dt_movingavg(d3.IC_r, 30)
s_IC_r /= std(s_IC_r, axis=0)
s_kin_reord_r = hstack([s_IC_r, d3.s_kin_r_noIC]) # sdt_kin_r[:, [0,20,21]], sdt_kin_r_noIC])
p1 = dot(d3.s_param_r.T, pinv(s_kin_reord_r.T))
#p2, _,_, _ = lstsq(sdt_kin_r, sdt_param_r, )
#p2 = p2.T
#allclose(p1, p2) # results in "True"
u,s,v = svd(p1, full_matrices = False)
# gram-schmidtify v
In [76]:
qv,rv = qr(v) # rv spans the same subspace as v
# TEST:
#prv = dot(rv.T, rv) # projector on "rv" subspace
#print allclose(prv, dot(prv, prv)) # True -> is a projector
#print allclose(dot(prv, v.T), v.T) # True -> projects into the same subspace as v
rv2 = rv.copy()
# make "identity" 3x3 block
rv2[1,:] -= rv2[2, :] * rv2[1,2] / rv2[2,2]
rv2[0,:] -= rv2[1, :] * rv2[0,1] / rv2[1,1]
rv2[0,:] -= rv2[2, :] * rv2[0,2] / rv2[2,2]
p2 = dot(rv2[3:,:].T, rv2[3:, :])
ap = eye(41) - p2
rv2[:3,:] = dot(ap,rv2[:3,:].T).T
figure()
pcolor(rv2)
colorbar()
clim(-1,1)
step 3: find minimal predictors
to content
In [78]:
# parameter to predict. select [0,1,2,4] for 2D-SLIP (excluding beta), or [0,1,2,3,4] for full 3D SLIP
p2D_r = sdt_param_r[:, [0,1,2,3,4]]
p2D_l = sdt_param_l[:, [0,1,2,3,4]]
In [92]:
Out[92]:
In [91]:
# Here, the main predictors ("directions") are computed. This is computationally quite fast
if exclude_IC_from_factors:
facs_r = st.find_factors(sdt_kin_r_noIC.T, p2D_r.T, k=n_factors)
facs_l = st.find_factors(sdt_kin_l_noIC.T, p2D_l.T, k=n_factors)
fscore_r = dot(facs_r.T, sdt_kin_r_noIC.T).T
fscore_l = dot(facs_l.T, sdt_kin_l_noIC.T).T
else:
facs_r = st.find_factors(sdt_kin_r.T, p2D_r.T, k=n_factors)
facs_l = st.find_factors(sdt_kin_l.T, p2D_l.T, k=n_factors)
# project data on lower dimensional subspace (do not take to full space again)
fscore_r = dot(facs_r.T, sdt_kin_r.T).T
fscore_l = dot(facs_l.T, sdt_kin_l.T).T
In [37]:
# reduced model: 6D state, 3 predictors
#reddat_r = hstack([fscore_r, dt_IC_r])
#reddat_l = hstack([fscore_l, dt_IC_l])
# form a model that *only* conists of parts of the "full" data (*excatly* the same data),
# which however may be projected to some axes
IC_r_from_dt = sdt_kin_r[:,[0, 20, 21]] # CoM height and horizontal plane velocities
IC_l_from_dt = sdt_kin_l[:,[0, 20, 21]]
reddat_r = hstack([fscore_r, IC_r_from_dt])
reddat_l = hstack([fscore_l, IC_l_from_dt])
# add data to section 3 data structure
d3.s_IC_r = IC_r_from_dt
d3.s_IC_l = IC_l_from_dt
# reddat_r = fscore_r
# reddat_l = fscore_l
In [38]:
# "select the initial conditions from full state information" - matrix
# this matrix only makes sense if exclude_IC_from_factors is set to False!
d3.display()
if not exclude_IC_from_factors:
IC_sel = zeros((41, 3))
IC_sel[0, 0] = 1
IC_sel[20, 1] = 1
IC_sel[21, 2] = 1
rm_selector_r = hstack([facs_r, IC_sel])
rm_selector_l = hstack([facs_l, IC_sel])
In [39]:
predict_oos = True
# select reduced models as a real subset of the full kinematic state at apex
if not exclude_IC_from_factors:
# use a real projection of the data!
reddat_r = dot(rm_selector_r.T, sdt_kin_r.T).T
reddat_l = dot(rm_selector_l.T, sdt_kin_l.T).T
#p2D_r = p2D_r - mean(p2D_r, axis=0)
#sdt_kin_r = sdt_kin_r - mean(sdt_kin_r, axis=0)
res1 = st.predTest(reddat_r, p2D_r, out_of_sample=predict_oos, rcond=1e-7)
res2 = st.predTest(sdt_kin_r, p2D_r, out_of_sample=predict_oos, rcond=1e-7)
res3 = st.predTest(reddat_l, p2D_l, out_of_sample=predict_oos, rcond=1e-7)
res4 = st.predTest(sdt_kin_l, p2D_l, out_of_sample=predict_oos, rcond=1e-7)
figure(figsize=(12,6))
subplot(1,2,1)
b1 = boxplot(res1)
b2 = boxplot(res2)
mi.recolor(b1, 'r')
mi.recolor(b2, 'k')
xticks(arange(5) + 1)
gca().set_xticklabels(['k', r'$\alpha$', r'L$_0$', r'$\beta$', r'$\delta$E'], fontsize=16)
xlabel('predicted variable')
ylabel('relative remaining variance')
ylim(0, 1.05)
title('right step SLIP params\nred: reduced model,\nblack: full kinematic state')
subplot(1,2,2)
b1 = boxplot(res3)
b2 = boxplot(res4)
mi.recolor(b1, 'r')
mi.recolor(b2, 'k')
xticks(arange(5) + 1)
gca().set_xticklabels(['k', r'$\alpha$', r'L$_0$', r'$\beta$', r'$\delta$E'], fontsize=16)
xlabel('predicted variable')
ylabel('relative remaining variance')
ylim(0, 1.05)
title('left step SLIP params\nred: reduced model,\nblack: full kinematic state')
draw()
step 3: find minimal predictors
to content
Note: to make a quick sanity check, you can change the "out-of-sample" prediction parameter to "False" to get in-sample prediction
In [40]:
# test self-consistency
oos_pred = True # out of sample prediction?
# predict
res1 = st.predTest(reddat_r, reddat_l, out_of_sample=oos_pred, rcond=1e-7)
res2 = st.predTest(sdt_kin_r, reddat_l, out_of_sample=oos_pred, rcond=1e-7)
res3 = st.predTest(reddat_l[:-1, :], reddat_r[1:, :], out_of_sample=oos_pred, rcond=1e-7)
res4 = st.predTest(sdt_kin_l[:-1, :], reddat_r[1:, :], out_of_sample=oos_pred, rcond=1e-7)
figure(figsize=(16,8))
subplot(1,2,1)
b1 = boxplot(res1)
b2 = boxplot(res2)
mi.recolor(b1, 'r')
mi.recolor(b2, 'k')
ylim(0, 1.05)
xticks(arange(res3.shape[1]) + 1)
gca().set_xticklabels(['factor ' + str(nr + 1) for nr in arange(res1.shape[1] - 3)] +
['height', 'horiz. speed', 'lat. speed'], rotation=45)
title('predicting left apex state\nred: reduced model, black: full model')
ylabel('relative remaining variance')
subplot(1,2,2)
b1 = boxplot(res3)
b2 = boxplot(res4)
mi.recolor(b1, 'r')
mi.recolor(b2, 'k')
ylim(0, 1.05)
xticks(arange(res3.shape[1]) + 1)
gca().set_xticklabels(['factor ' + str(nr + 1) for nr in arange(res1.shape[1] - 3)] +
['height', 'horiz. speed', 'lat. speed'], rotation=45)
title('predicting left apex state\nred: reduced model, black: full model')
ylabel('relative remaining variance')
draw()
In [41]:
# eigenvalue analyis
# full model
_, maps_1, _ = fda.fitData(sdt_kin_r, sdt_kin_l, nps=1, nrep=200, rcond=1e-7)
_, maps_2, _ = fda.fitData(sdt_kin_l[:-1, :], sdt_kin_r[1:, :], nps=1, nrep=200, rcond=1e-7, )
maps_full = [dot(m1, m2) for m1, m2 in zip(maps_1, maps_2)]
# reduced model
_, maps_1, _ = fda.fitData(reddat_r, reddat_l, nps=1, nrep=200, rcond=1e-7)
_, maps_2, _ = fda.fitData(reddat_l[:-1, :], reddat_r[1:, :], nps=1, nrep=200, rcond=1e-7, )
maps_red = [dot(m1, m2) for m1, m2 in zip(maps_1, maps_2)]
In [42]:
import fmalibs.util as ut
vi_full = ut.VisEig(127, False)
for A in maps_full:
vi_full.add(eig(A)[0])
vi_red = ut.VisEig(127, False)
for A in maps_red:
vi_red.add(eig(A)[0])
In [43]:
nps = 30 # number of sections per stride
map_sections = [0, 8, 15, 23] # select some sections for which mappings should be computed
map_sections = [0, 5, 10, 15, 20, 25] # select some sections for which mappings should be computed
#map_sections = [0, 7, 13, 19,25] # select some sections for which mappings should be computed
#map_sections = [0, 10, 20] # select some sections for which mappings should be computed
k_n = fda.dt_movingavg(k.make_1D(nps=nps)[:, 2 * nps:], 30) # exclude horizontal CoM position
k_n -= mean(k_n, axis=0)
k_n /= std(k_n, axis=0)
maps_pt, maps_full, idcs = fda.fitData(k_n[:-1, :], k_n[1:, :], nps=nps, sections=map_sections, nrep=200, rcond=1e-7 )
In [44]:
# compute stride maps from sections maps
# note: the correct order in the product of maps is map_n * map_(n-1) * ... * map_1
# otherwise, the resulting matrix is not a propagator
# -> reverse ordering of maps
all_maps = [reduce(dot, maps[::-1]) for maps in zip(*maps_pt)]
vi3 = ut.VisEig(127, False)
for A in all_maps:
vi3.add(eig(A)[0])
step 3: find minimal predictors
step 3.7.1: model 1 (x,y,vx,vy) both ankles
step 3.7.2: model 2 (x,y) both ankles, (vx, vy) swing leg (which swing leg?)
step 3.7.2: model 3 (x,y,vx,vy) of swing ankle only
to content
Shai would like to know the predictive ability with the states reduced as follows:
NOTE The state definition has changed: The absolute horizontal position has been removed.
On the one hand, this is because I'd expect a completely different behavior there (focussed not to leave the treadmill instead of getting back to some periodic orbit). On the other hand, if we include the absolute position in the state, the parameter calculation method is no longer valid. In fact, SLIP cannot match arbitrary data in all 6 CoM state space dimensions in a single step (Carver's result).
things to predict:
NOTE There are two different approaches to test the 1-stride autonomy:
NOTE The coordinate system here is:
x - lateral
y - anterior
z - vertical
In [19]:
import mutils.FDatAn as fda
import mutils.io as mio
import mutils.misc as mi
k3 = mio.KinData()
subject, ttype = 3, 1
k3.load(subject, ttype) # subject in [1,2,3,7], ttype=1
k3.selection = [ 'com_x', 'com_y', 'com_z',
'r_anl_y - com_y', 'r_anl_x - com_x', 'r_mtv_z - r_anl_z', 'r_mtv_x - r_anl_x', 'r_kne_y - com_y',
'r_elb_y - com_y', 'r_elb_x - com_x', 'r_wrl_z - com_z', 'r_wrl_x - com_x', 'cvii_y - com_y',
'l_anl_y - com_y', 'l_anl_x - com_x', 'l_mtv_z - l_anl_z', 'l_mtv_x - l_anl_x', 'l_kne_y - com_y',
'l_elb_y - com_y', 'l_elb_x - com_x', 'l_wrl_z - com_z', 'l_wrl_x - com_x',
'r_anl_z - com_z', 'l_anl_z - com_z',
'r_sia_y - l_sia_y']
SlipData = [mi.Struct(mio.mload('../data/2013-newSlip/SLIP_s%it%ir%i.dict' % (subject, ttype, rep)))
for rep in k3.reps]
kdata_full = build_dataset(k3, SlipData)
In [336]:
# common code: make predictions and create figures
import mutils.statistics as st
import mutils.misc as mi
# this function requires d3 - data object in its closure
def test_model(kdata_r, kdata_l, kdata_full, side="r"):
"""
This function performs the prediction tests and creates the
corrsponding figures displaying the results.
:args:
in_dat (nxd array): data to predict. *NOTE* it is assumed that the first 3 columns
represent the CoM state
side (str): "r" or "l", look at right or left step. *NOTE* currently ignored.
:returns:
fig (figure handle): a handle of the figure which will be created
"""
fig = figure(figsize=(17,7))
# identify indices of CoM in kdata_r - data
cidxr = []
for req_lbl in ['com_z', 'v_com_x', 'v_com_y']:
for idx, lbl in enumerate(kdata_r.kin_labels):
if lbl.lower() == req_lbl.lower():
cidxr.append(idx)
break
cidxr = array(cidxr)
print "indices of CoM in right submodel data:", cidxr
# identify indices of CoM in kdata_l - data
cidxl = []
for req_lbl in ['com_z', 'v_com_x', 'v_com_y']:
for idx, lbl in enumerate(kdata_l.kin_labels):
if lbl.lower() == req_lbl.lower():
cidxl.append(idx)
break
cidxl = array(cidxl)
print "indices of CoM in left submodel data:", cidxl
# make prediction for SLIP parameters
ax1 = fig.add_subplot(1,4,1)
pred1 = st.predTest(kdata_r.s_kin_r, kdata_r.s_param_r, rcond=1e-7)
pred2 = st.predTest(kdata_full.s_kin_r, kdata_r.s_param_r, rcond=1e-7)
pred3 = st.predTest(hstack([kdata_r.s_param_l[:-1,:], kdata_r.s_kin_r[1:, cidxr]]),
kdata_r.s_param_r[1:, ], rcond=1e-7) # there is no "data before first right apex"
b11 = ax1.boxplot(pred1, positions = arange(pred1.shape[1]) * .6 + .25)
mi.recolor(b11,'r')
b12 = ax1.boxplot(pred2, positions = arange(pred2.shape[1]) * .6 + .25)
mi.recolor(b12,'k')
b13 = ax1.boxplot(pred3, positions = arange(pred3.shape[1]) * .6 + .25)
mi.recolor(b13,'g')
ax1.set_title('prediction for SLIP parameters\nred: reduced model\nblack: full state information\ngreen: [CoM; last SLIP params]')
ax1.set_xticks(arange(pred1.shape[1]) * .6 + .25)
ax1.set_xticklabels(['$k$', '$\\alpha$', '$L_0$', '$\\beta$', '$\\Delta E$'])
ax1.set_yticks(.1 * arange(12))
ax1.plot(ax1.get_xlim(), [1, 1], 'k--')
ax1.set_ylim(0, 1.15)
# make "self consistency" prediction -> CoM state prediction is part of this
ax2 = fig.add_subplot(1,4,2)
pred1 = st.predTest(kdata_r.s_kin_r, kdata_l.s_kin_l, rcond=1e-7)
pred2 = st.predTest(kdata_full.s_kin_r, kdata_l.s_kin_l, rcond=1e-7)
pred3 = st.predTest(hstack([kdata_r.s_param_l[:-1,:], kdata_r.s_kin_r[1:, cidxr]]),
kdata_l.s_kin_l[1:, cidxl], rcond=1e-7) # there is no "data before first right apex"
b21 = ax2.boxplot(pred1, positions = arange(pred1.shape[1]) * .6 + .25)
mi.recolor(b21,'r')
b23 = ax2.boxplot(pred3, positions = cidxl * .6 + .25)
mi.recolor(b23,'g')
b22 = ax2.boxplot(pred2, positions = arange(pred2.shape[1]) * .6 + .25)
mi.recolor(b22,'k')
ax2.set_title('state prediction after 1 step \n(self-consistency)\nred: reduced model\nblack: full state information\ngreen: [CoM; last SLIP params]')
ax2.set_xticks(arange(pred2.shape[1]) * .6 + .25)
ax2.set_xticklabels(kdata_l.kin_labels, rotation='vertical')
ax2.set_yticks(.1 * arange(12))
ax2.plot(ax2.get_xlim(), [1, 1], 'k--')
ax2.set_ylim(0, 1.15)
# make "self consistency" prediction -> CoM state prediction is part of this
ax3 = fig.add_subplot(1,4,3)
pred1 = st.predTest(kdata_r.s_kin_r[:-1, :], kdata_r.s_kin_r[1:, :], rcond=1e-7)
pred2 = st.predTest(kdata_full.s_kin_r[:-1, :], kdata_r.s_kin_r[1:, :], rcond=1e-7)
pred3 = st.predTest(hstack([kdata_r.s_param_l[:-2,:], kdata_r.s_kin_r[1:-1, cidxr]]),
kdata_l.s_kin_r[2:, cidxr], rcond=1e-7) # there is no "data before first right apex"
b31 = ax3.boxplot(pred1, positions = arange(pred1.shape[1]) * .6 + .2)
mi.recolor(b31,'r')
b33 = ax3.boxplot(pred3, positions = cidxr * .6 + .25)
mi.recolor(b33,'g')
b32 = ax3.boxplot(pred2, positions = arange(pred2.shape[1]) * .6 + .225)
mi.recolor(b32,'k')
ax3.set_title('state prediction after 1 stride\n (self-consistency test)\nred: reduced model\nblack: full state information')
ax3.set_xticks(arange(pred2.shape[1]) * .6 + .25)
ax3.set_xticklabels(kdata_r.kin_labels, rotation='vertical')
ax3.set_yticks(.1 * arange(12))
ax3.plot(ax3.get_xlim(), [1, 1], 'k--')
ax3.set_ylim(0, 1.15)
# also: create two-stage prediction model: R->L->R instead of R->->R (directly)
# first: manually do two-stage prediction
# manually do the bootstrap
# define shortcuts
dat_r = kdata_r.s_kin_r
dat_l = kdata_l.s_kin_l
# 2 stages, reduced model
bs_vred = []
for rep in range(100):
idcs = randint(0, dat_r.shape[0] - 1, dat_r.shape[0] - 1)
oidx = fda.otheridx(idcs, dat_r.shape[0] - 1)
# regression R->L
A = dot(dat_l[idcs, :].T, pinv(dat_r[idcs, :].T, rcond=1e-7))
# regression L->R
B = dot(dat_r[idcs + 1, :].T, pinv(dat_l[idcs, :].T, rcond=1e-7))
pred = None # debug. dot(A, dat_r[oidx, :].T).T
pred_tot = reduce(dot, [B, A, dat_r[oidx, :].T]).T
vred = var(dat_r[oidx + 1, :] - pred_tot, axis=0) / var(dat_r[oidx + 1, :], axis=0)
bs_vred.append(vred)
bs_mdl = vstack(bs_vred)
# 2 stages, full model, select only which coordinates to display
# identify which components (indices) are taken in the small models:
idx_mdl = []
for idx in arange(kdata_r.s_kin_r.shape[1]):
for idxf in arange(kdata_full.s_kin_r.shape[1]):
if allclose(kdata_full.s_kin_r[:, idxf], kdata_r.s_kin_r[:, idx], atol=1e-7):
idx_mdl.append(idxf)
break
submdl_ident = len(idx_mdl) == kdata_r.s_kin_r.shape[1] # all corresponding dimensions found
if submdl_ident:
idx_mdl = array(idx_mdl)
print "corresponding dimensions of reduced and full model identified"
print "indices:", idx_mdl
dat_r = kdata_full.s_kin_r
dat_l = kdata_full.s_kin_l
bs_vred = []
for rep in range(100):
idcs = randint(0, dat_r.shape[0] - 1, dat_r.shape[0] - 1)
oidx = fda.otheridx(idcs, dat_r.shape[0] - 1)
# regression R->L
A = dot(dat_l[idcs, :].T, pinv(dat_r[idcs, :].T, rcond=1e-7))
# regression L->R
B = dot(dat_r[idcs + 1, :].T, pinv(dat_l[idcs, :].T, rcond=1e-7))
pred = None # dot(A, dat_r[oidx, :].T).T
pred_tot = reduce(dot, [B, A, dat_r[oidx, :].T]).T
vred = var(dat_r[oidx + 1, :] - pred_tot, axis=0) / var(dat_r[oidx + 1, :], axis=0)
bs_vred.append(vred)
bs_full = vstack(bs_vred)
# 2 stages, augmented SLIP model ( [CoM; last SLIP params] )
dat_r = hstack([ kdata_r.s_kin_r[1:, cidxr], kdata_r.s_param_l[:-1, :] ])
dat_l = hstack([ kdata_l.s_kin_l[1:, cidxl], kdata_l.s_param_r[1:, :] ])
bs_vred = []
for rep in range(100):
idcs = randint(0, dat_r.shape[0] - 1, dat_r.shape[0] - 1)
oidx = fda.otheridx(idcs, dat_r.shape[0] - 1)
# regression R->L
A = dot(dat_l[idcs, :].T, pinv(dat_r[idcs, :].T, rcond=1e-7))
# regression L->R
B = dot(dat_r[idcs + 1, :].T, pinv(dat_l[idcs, :].T, rcond=1e-7))
pred = None # debug. remove this line if no error occurs dot(A, dat_r[oidx, :].T).T
pred_tot = reduce(dot, [B, A, dat_r[oidx, :].T]).T
vred = var(dat_r[oidx + 1, :] - pred_tot, axis=0) / var(dat_r[oidx + 1, :], axis=0)
bs_vred.append(vred)
bs_augslip = vstack(bs_vred)[:, :3]
# display results
ax4 = fig.add_subplot(1,4,4)
b41 = ax4.boxplot(bs_mdl, positions = arange(bs_mdl.shape[1]) * .6 + .25)
mi.recolor(b41,'r')
b43 = ax4.boxplot(bs_augslip, positions = cidxr * .6 + .25)
mi.recolor(b43,'g')
if submdl_ident:
b42 = ax4.boxplot(bs_full[:, idx_mdl], positions = arange(len(idx_mdl)) * .6 + .275)
mi.recolor(b42,'k')
ax4.set_title('state prediction after 1 stride using\n two steps for prediction\nred: reduced model\nblack: full state information')
ax4.set_xticks(arange(bs_mdl.shape[1]) * .6 + .25)
ax4.set_xticklabels(kdata_r.kin_labels, rotation='vertical')
ax4.set_yticks(.1 * arange(12))
ax4.plot(ax4.get_xlim(), [1, 1], 'k--')
ax4.set_ylim(0, 1.15)
return fig
step 3: find minimal predictors
step 3.7: test different explicit models
to content
NOTE for the full Floquet model, the prediction quality increases when the number of sections that describe one stride is increased. As expected, this does not hold true for in-sample predictions.
In [333]:
#k3.selection = ['com_x', 'com_y', 'com_z', 'r_anl_y - com_y', 'l_anl_y - com_y', 'r_anl_z - com_z', 'l_anl_z - com_z', ]
#kdata = build_dataset(k3, SlipData)
#fig = test_model(kdata, kdata, kdata_full)
In [337]:
k3.selection = ['com_x', 'com_y', 'com_z', 'r_anl_y - com_y', 'l_anl_y - com_y', 'r_anl_z - com_z', 'l_anl_z - com_z']
kdata = build_dataset(k3, SlipData)
fig = test_model(kdata, kdata, kdata_full)
step 3: find minimal predictors
step 3.7: test different explicit models
to content
In [49]:
# variant 1: remove swing leg velocities
k3.selection = ['com_x', 'com_y', 'com_z', 'r_anl_y - com_y', 'r_anl_z - com_z', 'l_anl_y - com_y', 'l_anl_z - com_z']
kdata_r = build_dataset(k3, SlipData)
k3.selection = ['com_x', 'com_y', 'com_z', 'l_anl_y - com_y', 'l_anl_z - com_z', 'r_anl_y - com_y', 'r_anl_z - com_z']
kdata_l = build_dataset(k3, SlipData)
# manually remove velocity of swing leg from data
kdata_r.s_kin_r = kdata_r.s_kin_r[:, :-2]
kdata_r.kin_labels = kdata_r.kin_labels[:-2]
kdata_l.s_kin_l = kdata_l.s_kin_l[:, :-2]
kdata_l.kin_labels = kdata_l.kin_labels[:-2]
fig = test_model(kdata_r, kdata_l, kdata_full)
In [14]:
# variant 2: remove stance leg velocities
k3.selection = ['com_x', 'com_y', 'com_z', 'l_anl_y - com_y', 'l_anl_z - com_z', 'r_anl_y - com_y', 'r_anl_z - com_z']
kdata_r = build_dataset(k3, SlipData)
k3.selection = ['com_x', 'com_y', 'com_z', 'r_anl_y - com_y', 'r_anl_z - com_z', 'l_anl_y - com_y', 'l_anl_z - com_z']
kdata_l = build_dataset(k3, SlipData)
# manually remove velocity of swing leg from data
kdata_r.s_kin_r = kdata_r.s_kin_r[:, :-2]
kdata_r.kin_labels = kdata_r.kin_labels[:-2]
kdata_l.s_kin_l = kdata_l.s_kin_l[:, :-2]
kdata_l.kin_labels = kdata_l.kin_labels[:-2]
fig = test_model(kdata_r, kdata_l, kdata_full)
step 3: find minimal predictors
step 3.7: test different explicit models
to content
In [51]:
# here: use only information from leg that will touchdown next
k3.selection = ['com_x', 'com_y', 'com_z', 'r_anl_y - com_y', 'r_anl_z - com_z', ]
kdata_r = build_dataset(k3, SlipData)
k3.selection = ['com_x', 'com_y', 'com_z', 'l_anl_y - com_y', 'l_anl_z - com_z', ]
kdata_l = build_dataset(k3, SlipData)
fig = test_model(kdata_r, kdata_l, kdata_full)
In [338]:
# here: use only information from leg that will touchdown next
k3.selection = ['com_x', 'com_y', 'com_z', 'r_anl_y - com_y', 'r_anl_z - com_z', 'r_anl_x - com_x',] #'r_sia_y - l_sia_y'] # add hip rotation indicator?
kdata_r = build_dataset(k3, SlipData)
k3.selection = ['com_x', 'com_y', 'com_z', 'l_anl_y - com_y', 'l_anl_z - com_z', 'l_anl_x - com_x',] #'r_sia_y - l_sia_y'] # add hip rotation indicator?
kdata_l = build_dataset(k3, SlipData)
fig = test_model(kdata_r, kdata_l, kdata_full)
In [53]:
# interchange stance leg <-> swing leg
fig = test_model(kdata_l, kdata_r, kdata_full)
step 4.1: find periodic solution
step 4.2: compute parameter prediction maps
step 4.3: define stride function
step 4.4: compute jacobian
to content
The closed loop model reads as follows:
Step 1 ("right"):
state = [IC; Factors] where IC = initial CoM state at apex
params = P1 * state where P is a regression from data
new_state = [ode(IC, params); Q1 * [IC; Factors]] where Q is a regressed map from data
Step 2 ("left"):
state = [IC; Factors] where IC = initial CoM state at apex
params = P2 * state where P is a regression from data
new_state = [ode(IC, params); Q2 * [IC; Factors]] where Q is a regressed map from data
step 4: create controlled SLIP
to content
Approach 1: find periodic solution that corresponds to the average parameters, and verify that it's close to the average initial conditions.
Approach 2: find periodic solution that corresponds to the average initial conditions (and time and ymin), and verify that the corresponding parameters are close to the average parameters
to select approach 1 or 2, go to the "init" block at the top of this notebook
In [57]:
k4 = ws9.k # or any other dataset from build_dataset
po_average_over_IC = True
import models.sliputil as su
mean_pr = mean(vstack(k4.all_param_r), axis=0)
mean_pl = mean(vstack(k4.all_param_l), axis=0) #param_left), axis=0)
mean_ICr = mean(k4.all_IC_r, axis=0)
mean_ICl = mean(k4.all_IC_l, axis=0)
mass = mean(k4.masses)
if not po_average_over_IC: # average parameters, compute corresponding periodic solution
mean_pl[4] = -mean_pr[4] # set total energy change to exactly zero.
# Note: typically, the difference is low, roughly ~0.01 J
ICp_r, ICp_l = su.getPeriodicOrbit_p(mean_pr, mean_pl, aug_dict={'m': mass, 'g' : -9.81}, startState=mean_ICr)
else: # average initial conditions, compute corresponding periodic solution
[ICp_r, Pp_r, dE_r], [ICp_l, Pp_l, dE_l] = su.getPeriodicOrbit(k4.all_IC_r, vstack(k4.TR), vstack(k4.yminR),
k4.all_IC_l, vstack(k4.TL), vstack(k4.yminL), baseParams ={'m': mass, 'g' : -9.81}, startParams=mean(vstack(k4.all_param_r), axis=0)[:4])
# change last element to be energy input - this is consistent with the format used in the rest of the code
Pp_r = array(Pp_r)
Pp_l = array(Pp_l)
Pp_r[4] = dE_r
Pp_r = Pp_r[:5]
Pp_l[4] = dE_l
Pp_l = Pp_l[:5]
# rename "periodic parameters" such that they are used in subsequent code cells
mean_pr = Pp_r
mean_pl = Pp_l
In [60]:
#verify periodicity
sr1 = su.finalState(ICp_r, mean_pr, addDict = {'m' : mass, 'g' : -9.81})
sl1 = su.finalState(ICp_l, mean_pl, addDict = {'m' : mass, 'g' : -9.81})
print "difference between identified periodic orbit and simulation results:"
print sr1 - ICp_l
print sl1 - ICp_r
print '\n===========================================================\n'
print "Deviation from periodic orbit to average apex state, in units of standard deviations:"
print " [height, horizontal speed, lateral speed]"
print "left step: ", (ICp_l - mean_ICl) / std(k4.all_IC_l, axis=0)
print "right step:", (ICp_r - mean_ICr) / std(k4.all_IC_r, axis=0)
print '\n===========================================================\n'
print "relative deviation from periodic parameters to mean parameters,"
print "in units of standard deviation of parameters"
print "[k, alpha, l0, beta, deltaE]"
print (mean_pr - mean(k4.all_param_r, axis=0)) / std(k4.all_param_r, axis=0)
print (mean_pl - mean(k4.all_param_l, axis=0)) / std(k4.all_param_l, axis=0)
In [65]:
print mean(vstack(k4.IC_right), axis=0) #display()
print k4.all_IC_r.shape
print vstack(k4.IC_right).shape
mean(k4.all_IC_r, axis=0)
#[sx.vBelt for sx in SlipData]
Out[65]:
In [66]:
# new: moved to module
from models.sliputil import getControlMaps
In [93]:
# old code
dt_fstate_r = hstack([fda.dt_movingavg(k4.all_IC_r, 30), fscore_r])
dt_fstate_l = hstack([fda.dt_movingavg(k4.all_IC_l, 30), fscore_l])
dt_fstate_r -= mean(dt_fstate_r, axis=0)
dt_fstate_l -= mean(dt_fstate_l, axis=0)
# compute parameter prediction maps
_, all_ctrl_r, _ = fda.fitData(dt_fstate_r, dt_param_r, nps=1, nrep=200, sections=[0,], rcond=1e-8)
_, all_ctrl_l, _ = fda.fitData(dt_fstate_l, dt_param_l, nps=1, nrep=200, sections=[0,], rcond=1e-8)
ctrl_r = fda.meanMat(all_ctrl_r)
ctrl_l = fda.meanMat(all_ctrl_l)
# compute factors prediction maps - "propagators" of factor's state
_, all_facprop_r, _ = fda.fitData(dt_fstate_r, fscore_l, nps=1, nrep=200, sections=[0,], rcond=1e-8)
_, all_facprop_l, _ = fda.fitData(dt_fstate_l[:-1, :], fscore_r[1:, :], nps=1, nrep=200, sections=[0,], rcond=1e-8)
facprop_r = fda.meanMat(all_facprop_r)
facprop_l = fda.meanMat(all_facprop_l)
step 4: create controlled SLIP
to content
Note that this function does not depend on the actual shape of the factors, i.e. if there are 3 or 5 factors.
In [94]:
from models.sliputil import get_auto_sys
allow_nonzero_ref = 1 # set this to zero to force "zero" as reference for the fscores!
#mean_ICl
refstate_r = hstack([ICp_r, allow_nonzero_ref * mean(fscore_r, axis=0)]) # the latter should be zero anyway
refstate_l = hstack([ICp_l, allow_nonzero_ref * mean(fscore_l, axis=0)]) # the latter should be zero anyway
# f is now an autonomous, controlled SLIP stride function
f = get_auto_sys(mean_pr, mean_pl, refstate_r, refstate_l, ctrl_r, ctrl_l, facprop_r, facprop_l, {'m': mass, 'g':-9.81})
In [95]:
f(refstate_l)
Out[95]:
In [ ]:
# test sensitivity. This is bugfixing prior to computation of Jacobian.
h = .0001
elem = 0
IC = refstate_r.copy()
IC[elem] += h
fsp = f(IC)
IC[elem] -= 2*h
fsn = f(IC)
set_printoptions(precision=4)
print array(fsp - refstate_r) / (2.*h)
print array(fsn - refstate_r) / (2.*h)
step 4: create controlled SLIP
to content
also, visualize eigenvalues and eigenvalues of the "reduced" discrete model
In [97]:
J = []
h = .001
for elem in range(len(refstate_r)):
IC = refstate_r.copy()
IC[elem] += h
fsp = f(IC)
IC[elem] -= 2*h
fsn = f(IC)
J.append((fsp - fsn) / (2.*h))
J = vstack(J).T
#visualize
figure()
for ev in eig(J)[0]:
plot(ev.real, ev.imag, 'rd')
axis('equal')
vi_red.vis1()
xlim(-1,1)
ylim(-1,1)
title('eigenvalues of reduced model vs. controlled SLIP model')
In [ ]:
# compute stride maps from sections maps
# note: the correct order in the product of maps is map_n * map_(n-1) * ... * map_1
# otherwise, the resulting matrix is not a propagator
# -> reverse ordering of maps
all_maps = [reduce(dot, maps[::-1]) for maps in zip(*maps_pt)]
vi3 = ut.VisEig(127, False)
for A in all_maps:
vi3.add(eig(A)[0])
figure(figsize=(18, 6))
subplot(1,3,1)
vr = vi_full.vis1()[0]
vr.set_cmap(cm.jet)
vr = vi_red.vis1()[0]
vr.set_cmap(cm.hot)
xlim(-1,1)
ylim(-1,1)
title('eigenvalue distribution "reduced model" (red)\n vs. full model (sections at apex)')
subplot(1,3,2)
vr = vi3.vis1()[0]
vr.set_cmap(cm.jet)
vr = vi_red.vis1()[0]
vr.set_cmap(cm.hot)
xlim(-1,1)
ylim(-1,1)
title('eigenvalues of full kinematic model\nwith %i sections per stride\n vs. reduced model(red)'
% len(map_sections))
xlabel('real part')
ylabel('imaginary part')
subplot(1,3,3)
vr = vi3.vis1()[0]
vr.set_cmap(cm.jet)
vr = vi_red.vis1()[0]
vr.set_cmap(cm.hot)
for ev in eig(J)[0]:
plot(ev.real, ev.imag, 'kd')
xlim(-1,1)
ylim(-1,1)
title('\n'.join(['eigenvalues of full kinematic model',
'with %i sections per stride', 'vs. reduced model(red)',
'vs. forward model (diamonds)'])
% len(map_sections))
xlabel('real part')
ylabel('imaginary part')
draw()
In [ ]:
mag_thresh = .33
#mag_thresh = 0
facs_l_large = array(abs(facs_l) > mag_thresh) * facs_l
facs_r_large = array(abs(facs_r) > mag_thresh) * facs_r
figure(figsize=(15,4))
subplot(2,1,1)
pcolor(facs_l_large.T)
colorbar()
clim(-1,1)
xlim(0,41)
gca().set_yticks(arange(3) + .5)
gca().set_yticklabels([])
gca().set_xticks(arange(41) + .5)
gca().set_xticklabels([])
ylabel('for left leg')
title('elements of factors for predicting SLIP parameters, thresholded')
subplot(2,1,2)
pcolor(facs_r_large.T)
colorbar()
lbls = k.selection[2:] + [('v_' + pt) for pt in k.selection[:2] + k.selection[3:]]
gca().set_xticks(arange(41) + .5)
gca().set_xticklabels(lbls, rotation=90)
gca().set_yticks(arange(3) + .5)
ylabel('for right leg')
gca().set_yticklabels([])
clim(-1,1)
xlim(0,41)
print len(lbls)
"rotate" factors such that the same subspace is spanned by them, however minimizing the L1-norm Q: is this still relevant?
approach would be (idea):
In [ ]:
# test autonomy of system [IC; factor 2; factor 3]
# test autonomus systems: ankle-SLIP unilateral / bilateral
# find good reduced EXPLICIT model!
# can we rotate the 5-factor state to COM-state + 2 factors?
In [ ]:
# define dimensionality of new model. NOTE: must be <= n_factors
mdl_size = 2
p_IC = dot(IC_sel, IC_sel.T) # projector on IC
p_rem = eye(p_IC.shape[0]) - p_IC # 1 - projector on IC
f_reduced = dot(p_rem, facs_r)
u, s, v = svd(f_reduced, full_matrices=False)
facs_r_noIC = u[:, :mdl_size].copy()
f_reduced = dot(p_rem, facs_l)
u, s, v = svd(f_reduced, full_matrices=False)
facs_l_noIC = u[:, :mdl_size].copy()
# use this to select ankle-SLIP
if select_ankle_SLIP:
nr_dim = 8 # select 4 or 8
# === HUGE ankle model: +8 states ====
facs_r_noIC = zeros((41,nr_dim))
facs_l_noIC = zeros((41,nr_dim))
# select right ankles
facs_r_noIC[1,0] = facs_r_noIC[2, 1] = facs_r_noIC[22,2] = facs_r_noIC[23,3] = 1 # right ankle states
if nr_dim == 8:
facs_r_noIC[11,4] = facs_r_noIC[12, 5] = facs_r_noIC[32,6] = facs_r_noIC[33,7] = 1 # left ankle states
# select left ankles
facs_l_noIC[11,0] = facs_l_noIC[12, 1] = facs_l_noIC[32,2] = facs_l_noIC[33,3] = 1 # left ankle states
if nr_dim == 8:
facs_l_noIC[1,4] = facs_l_noIC[2, 5] = facs_l_noIC[22,6] = facs_l_noIC[23,7] = 1 # right ankle states
# projector onto the subspace of the model
rm_selector_l = hstack([facs_l_noIC, IC_sel])
rm_selector_r = hstack([facs_r_noIC, IC_sel])
# the scores on the "non-IC" states
fscore_r_noIC = dot(facs_r_noIC.T, sdt_kin_r.T).T
fscore_l_noIC = dot(facs_l_noIC.T, sdt_kin_l.T).T
In [ ]:
predict_oos = True # predict out of sample?
# select reduced models as a real subset of the full kinematic state at apex
reddat_r = dot(rm_selector_r.T, sdt_kin_r.T).T
reddat_l = dot(rm_selector_l.T, sdt_kin_l.T).T
#p2D_r = p2D_r - mean(p2D_r, axis=0)
#sdt_kin_r = sdt_kin_r - mean(sdt_kin_r, axis=0)
res1 = st.predTest(reddat_r, p2D_r, out_of_sample=predict_oos, rcond=1e-7)
res2 = st.predTest(sdt_kin_r, p2D_r, out_of_sample=predict_oos, rcond=1e-7)
res3 = st.predTest(reddat_l, p2D_l, out_of_sample=predict_oos, rcond=1e-7)
res4 = st.predTest(sdt_kin_l, p2D_l, out_of_sample=predict_oos, rcond=1e-7)
figure(figsize=(12,6))
subplot(1,2,1)
b1 = boxplot(res1)
b2 = boxplot(res2)
mi.recolor(b1, 'g')
mi.recolor(b2, 'k')
xticks(arange(5) + 1)
gca().set_xticklabels(['k', r'$\alpha$', r'L$_0$', r'$\beta$', r'$\delta$E'], fontsize=16)
xlabel('predicted variable')
ylabel('relative remaining variance')
ylim(0, 1.05)
title('right step SLIP params\nred: reduced model,\nblack: full kinematic state')
subplot(1,2,2)
b1 = boxplot(res3)
b2 = boxplot(res4)
mi.recolor(b1, 'g')
mi.recolor(b2, 'k')
xticks(arange(5) + 1)
gca().set_xticklabels(['k', r'$\alpha$', r'L$_0$', r'$\beta$', r'$\delta$E'], fontsize=16)
xlabel('predicted variable')
ylabel('relative remaining variance')
ylim(0, 1.05)
title('left step SLIP params\nred: reduced model,\nblack: full kinematic state')
draw()
In [ ]:
# test self-consistency
oos_pred = True # out of sample prediction?
# predict
res1 = st.predTest(reddat_r, reddat_l, out_of_sample=oos_pred, rcond=1e-7)
res2 = st.predTest(sdt_kin_r, reddat_l, out_of_sample=oos_pred, rcond=1e-7)
res3 = st.predTest(reddat_l[:-1, :], reddat_r[1:, :], out_of_sample=oos_pred, rcond=1e-7)
res4 = st.predTest(sdt_kin_l[:-1, :], reddat_r[1:, :], out_of_sample=oos_pred, rcond=1e-7)
figure(figsize=(16,8))
subplot(1,2,1)
b1 = boxplot(res1)
b2 = boxplot(res2)
mi.recolor(b1, 'g')
mi.recolor(b2, 'k')
ylim(0, 1.05)
xticks(arange(res3.shape[1]) + 1)
gca().set_xticklabels(['factor ' + str(nr + 1) for nr in arange(res1.shape[1] - 3)] +
['height', 'horiz. speed', 'lat. speed'], rotation=45)
title('predicting left apex state\nred: reduced model, black: full model')
ylabel('relative remaining variance')
subplot(1,2,2)
b1 = boxplot(res3)
b2 = boxplot(res4)
mi.recolor(b1, 'g')
mi.recolor(b2, 'k')
ylim(0, 1.05)
xticks(arange(res3.shape[1]) + 1)
gca().set_xticklabels(['factor ' + str(nr + 1) for nr in arange(res1.shape[1] - 3)] +
['height', 'horiz. speed', 'lat. speed'], rotation=45)
title('predicting left apex state\nred: reduced model, black: full model')
ylabel('relative remaining variance')
draw()
Step 8.1: visualize eigenvalues of different models
to content
In [ ]:
# compute propagator matrices
# introduce scaling such that "scores" and "IC" variance is in the same order of magnitude
score_scaling = .04
# first: initialize data
dt_fstate_r_noIC = hstack([fda.dt_movingavg(all_IC_r, 30), fscore_r_noIC * score_scaling])
dt_fstate_l_noIC = hstack([fda.dt_movingavg(all_IC_l, 30), fscore_l_noIC * score_scaling])
dt_fstate_r_noIC -= mean(dt_fstate_r_noIC, axis=0)
dt_fstate_l_noIC -= mean(dt_fstate_l_noIC, axis=0)
#dt_fstate_l_noIC *= .04
#dt_fstate_r_noIC *= .04
# compute parameter prediction maps
_, all_ctrl_r_noIC, _ = fda.fitData(dt_fstate_r_noIC, dt_param_r, nps=1, nrep=200, sections=[0,], rcond=1e-8)
_, all_ctrl_l_noIC, _ = fda.fitData(dt_fstate_l_noIC, dt_param_l, nps=1, nrep=200, sections=[0,], rcond=1e-8)
ctrl_r_noIC = fda.meanMat(all_ctrl_r_noIC)
ctrl_l_noIC = fda.meanMat(all_ctrl_l_noIC)
# compute factors prediction maps - "propagators" of factor's state
#_, all_facprop_r_noIC, _ = fda.fitData(dt_fstate_r_noIC, fscore_l_noIC, nps=1, nrep=200, sections=[0,], rcond=1e-8)
#_, all_facprop_l_noIC, _ = fda.fitData(dt_fstate_l_noIC[:-1, :], fscore_r_noIC[1:, :], nps=1, nrep=200, sections=[0,], rcond=1e-8)
_, all_facprop_r_noIC, _ = fda.fitData(dt_fstate_r_noIC, score_scaling * fscore_l_noIC, nps=1, nrep=200, sections=[0,], rcond=1e-8)
_, all_facprop_l_noIC, _ = fda.fitData(dt_fstate_l_noIC[:-1, :], score_scaling * fscore_r_noIC[1:, :], nps=1, nrep=200, sections=[0,], rcond=1e-8)
facprop_r_noIC = fda.meanMat(all_facprop_r_noIC)
facprop_l_noIC = fda.meanMat(all_facprop_l_noIC)
In [ ]:
from models.sliputil import get_auto_sys
allow_nonzero_ref = 0 # set this to zero to force "zero" as reference for the fscores!
#mean_ICl
refstate_r_noIC = hstack([ICp_r, allow_nonzero_ref * mean(fscore_r_noIC, axis=0)]) # the latter should be zero anyway
refstate_l_noIC = hstack([ICp_l, allow_nonzero_ref * mean(fscore_l_noIC, axis=0)]) # the latter should be zero anyway
f_noIC = get_auto_sys(mean_pr, mean_pl, refstate_r_noIC, refstate_l_noIC, ctrl_r_noIC, ctrl_l_noIC, facprop_r_noIC, facprop_l_noIC, {'m': mass, 'g':-9.81})
#IC = refstate_r
J_noIC = []
h = .001
#h = .001
for elem in range(len(refstate_r_noIC)):
IC = refstate_r_noIC.copy()
IC[elem] += h
fsp = f_noIC(IC)
IC[elem] -= 2*h
fsn = f_noIC(IC)
J_noIC.append((fsp - fsn) / (2.*h))
J_noIC = vstack(J_noIC).T
In [ ]:
mag_thresh = .1
#mag_thresh = 0
facs_l_large = array(abs(facs_l_noIC) > mag_thresh) * facs_l_noIC
facs_r_large = array(abs(facs_r_noIC) > mag_thresh) * facs_r_noIC
figure(figsize=(15,3))
subplot(2,1,1)
pcolor(facs_l_large.T)
colorbar()
clim(-1,1)
xlim(0,41)
gca().set_yticks(arange(facs_r_noIC.shape[1]) + .5)
gca().set_yticklabels([])
gca().set_xticks(arange(41) + .5)
gca().set_xticklabels([])
ylabel('for left leg')
title('elements of factors for predicting SLIP parameters, thresholded')
subplot(2,1,2)
pcolor(facs_r_large.T)
colorbar()
lbls = k.selection[2:] + [('v_' + pt) for pt in k.selection[:2] + k.selection[3:]]
gca().set_xticks(arange(41) + .5)
gca().set_xticklabels(lbls, rotation=90)
gca().set_yticks(arange(facs_r_noIC.shape[1]) + .5)
ylabel('for right leg')
gca().set_yticklabels([])
clim(-1,1)
xlim(0,41)
In [ ]:
# compute stride maps from sections maps
# note: the correct order in the product of maps is map_n * map_(n-1) * ... * map_1
# otherwise, the resulting matrix is not a propagator
# -> reverse ordering of maps
all_maps = [reduce(dot, maps[::-1]) for maps in zip(*maps_pt)]
vi3 = ut.VisEig(127, False)
for A in all_maps:
vi3.add(eig(A)[0])
figure(figsize=(18, 6))
subplot(1,3,1)
vr = vi_full.vis1()[0]
vr.set_cmap(cm.jet)
vr = vi_red.vis1()[0]
vr.set_cmap(cm.hot)
xlim(-1,1)
ylim(-1,1)
title('eigenvalue distribution "reduced model" (red)\n vs. full model (sections at apex)')
subplot(1,3,2)
vr = vi3.vis1()[0]
vr.set_cmap(cm.jet)
vr = vi_red.vis1()[0]
vr.set_cmap(cm.hot)
xlim(-1,1)
ylim(-1,1)
title('eigenvalues of full kinematic model\nwith %i sections per stride\n vs. reduced model(red)'
% len(map_sections))
xlabel('real part')
ylabel('imaginary part')
subplot(1,3,3)
vr = vi3.vis1()[0]
vr.set_cmap(cm.gray)
vr = vi_red.vis1()[0]
vr.set_cmap(cm.hot)
for ev in eig(J)[0]:
plot(ev.real, ev.imag, 'kd', markersize=9)
for ev in eig(J_noIC)[0]:
plot(ev.real, ev.imag, 'v', color='#24ff49', markersize=7.5)
xlim(-1,1)
ylim(-1,1)
if select_ankle_SLIP:
last_mdl = ' ankle-SLIP '
else:
last_mdl = '"no-IC-factors" forward model '
title('\n'.join(['eigenvalues of full kinematic model (gray)',
'with %i sections per stride', 'vs. reduced model(red)',
'vs. forward model (black diamonds)',
'vs. ' + last_mdl + ' (green triangles)'])
% len(map_sections))
xlabel('real part')
ylabel('imaginary part')
draw()
In [ ]:
# compute 5 factors
# let's assume that the first 3 components are CoM states
# use Gram-Schmidt to make first three cols "unity matrix" (5x3)
# (use Gram-Schmidt to) minimize the L2-norm of the upper right 3x38-factor matrix
# alternatively:
# start with 3x3 - identity (+ 3x38)
# add two [0 0 0, 38x] vectors to minimize the prediction error in parameters (explicit formula? iterative?)
# alternatively: re-work my original SVD approach
In [ ]:
ev, ew = eig(J_noIC)
figure('EV dist', figsize=(16,6))
subplot(1,2,1)
plot(abs(ev), 'rd')
xlabel('ordinal of eigenvalue')
ylabel('magnitude of eigenvalue')
subplot(1,2,2)
pcolor(abs(ew[::-1,:]))
gca().set_xticks((arange(ew.shape[0]) + .5))
gca().set_xticklabels((arange(ew.shape[0]) + 1))
gca().set_yticks((arange(ew.shape[0]) + .5))
gca().set_yticklabels((arange(ew.shape[0]) + 1))
xlim(0,ew.shape[0])
ylim(ew.shape[0], 0)
clim(0,1)
colorbar()
title('magnitude of eigenvector matrix')
xlabel('ordinal of eigenvector')
ylabel('coordinate [1-3: CoM]')
In [ ]:
figure()
pcolor(
vstack([abs(ew[:,col]) / std(dt_fstate_r_noIC, axis=0) for col in arange(ew.shape[1])]).T
)
colorbar()
title('"scaled" eigenvector matix')
Step 9.1: Select and prepare data
Step 9.2: create bootstrapped Floquet models
Step 9.3: test continous prediction for Floquet models
Step 9.4: (now obsolete)
Step 9.5: compute Phase for SLIP
Step 9.6: perform predictions
to content
to compare:
CoM (x|y|z)
for continuous data with phase:
In [1]:
import mutils.io as mio
import mutils.misc as mi
import mutils.FDatAn as fda
from copy import deepcopy
from mutils.io import build_dataset
from models.sliputil import get_auto_sys, getControlMaps
ws9 = mio.saveable()
k9 = mio.KinData()
ws9.subject = 2
ws9.ttype = 1
k9.load(ws9.subject, ws9.ttype) # subject in [1,2,3,7], ttype=1
SlipData = [mi.Struct(mio.mload('../data/2013-newSlip/SLIP_s%it%ir%i.dict' % (ws9.subject, ws9.ttype, rep)))
for rep in k9.reps]
k9.selection = ['com_x', 'com_y', 'com_z', 'r_anl_y - com_y', 'r_anl_z - com_z', 'r_anl_x - com_x']
ws9.dataset_r = build_dataset(k9, SlipData)
k9.selection = ['com_x', 'com_y', 'com_z', 'l_anl_y - com_y', 'l_anl_z - com_z', 'l_anl_x - com_x']
ws9.dataset_l = build_dataset(k9, SlipData)
ws9.stride_idcs = [1200, 1201, 1202, 1203] # numbers of strides to predict
ws9.k9 = mio.KinData()
ws9.k9.load(ws9.subject, ws9.ttype) # subject in [1,2,3,7], ttype=1
ws9.k9.selection = [ 'com_x', 'com_y', 'com_z',
'r_anl_y - com_y', 'r_anl_x - com_x', 'r_mtv_z - r_anl_z', 'r_mtv_x - r_anl_x', 'r_kne_y - com_y',
'r_elb_y - com_y', 'r_elb_x - com_x', 'r_wrl_z - com_z', 'r_wrl_x - com_x', 'cvii_y - com_y',
'l_anl_y - com_y', 'l_anl_x - com_x', 'l_mtv_z - l_anl_z', 'l_mtv_x - l_anl_x', 'l_kne_y - com_y',
'l_elb_y - com_y', 'l_elb_x - com_x', 'l_wrl_z - com_z', 'l_wrl_x - com_x',
'r_anl_z - com_z', 'l_anl_z - com_z']
ws9.SlipData = [mi.Struct(mio.mload('../data/2013-newSlip/SLIP_s%it%ir%i.dict' % (ws9.subject, ws9.ttype, rep)))
for rep in ws9.k9.reps]
ws9.kdata_full = build_dataset(ws9.k9, ws9.SlipData)
ws9.k = ws9.kdata_full
In [2]:
ws9.a_phi_apex_r = mod(hstack([mi.upper_phases(d.phases[:-1], sep=0) for d in ws9.SlipData]), 2*pi)
ws9.a_phi_apex_l = mod(hstack([mi.upper_phases(d.phases[:-1], sep=pi) for d in ws9.SlipData]), 2*pi)
ws9.phi_apex_r = mean(ws9.a_phi_apex_r)
ws9.phi_apex_l = mean(ws9.a_phi_apex_l)
ws9.s_phi_apex_r = std(ws9.a_phi_apex_r)
ws9.s_phi_apex_l = std(ws9.a_phi_apex_l)
figure(figsize=(6,4))
hist(ws9.a_phi_apex_r)
xlabel('phases of "right apex" events')
ylabel('appearances')
print "apex right at phase: ", ws9.phi_apex_r, ' +- ', ws9.s_phi_apex_r
print "apex left at phase: ", ws9.phi_apex_l, ' +- ', ws9.s_phi_apex_l
print "difference [in units of pi]:", (ws9.phi_apex_l - ws9.phi_apex_r) / pi
step 9: compare trajectories SLIP vs. Floquet models
to content
In [3]:
# prepare data
# get kinematics at different phases - interpolate using k.i_phases
ws9.nps = 50
#the final [:, nps*2:] removes the com position in horizontal plane
ws9.d1d_r = ws9.k9.make_1D(nps=ws9.nps, phases_list=ws9.k.all_phases_r)[:, ws9.nps*2:] # starts at "right" apex
ws9.phases_d1d_r = deepcopy(ws9.k9.i_phases)
ws9.d1d_l = ws9.k9.make_1D(nps=ws9.nps, phases_list=ws9.k.all_phases_l)[:, ws9.nps*2:] # starts at "left" apex
ws9.phases_d1d_l = deepcopy(ws9.k9.i_phases)
detrend = lambda x: fda.dt_movingavg(x, 30)
ws9.d1d_r_dt = detrend(ws9.d1d_r)
ws9.d1d_r_trend = ws9.d1d_r - ws9.d1d_r_dt #required for offset ("lift to limit cycle")
ws9.d1d_l_dt = detrend(ws9.d1d_l)
ws9.d1d_l_trend = ws9.d1d_l - ws9.d1d_l_dt # required for offset ("lift to limit cycle")
# create Floquet models, bootstrap them
ws9.n_sct = ws9.nps // 2 # each floquet model is only 25 sections "long" - nps / 2
ws9.fmodels_r = []
print "computing Floquet models for", ws9.n_sct, "sections... (right)"
print "=" * ws9.n_sct
for sec in arange(ws9.n_sct) + 1: # from section 1 to "next apex - 1"
_, maps_sect, _ = fda.fitData( ws9.d1d_r_dt[:, 0::ws9.nps], ws9.d1d_r_dt[:, sec::ws9.nps],
sections=[0,], nps=1, rcond=1e-7, nrep=50)
ws9.fmodels_r.append(fda.meanMat(maps_sect))
sys.stdout.write('.')
print " done"
ws9.fmodels_l = []
print "computing Floquet models for", ws9.n_sct, "sections... (left)"
print "=" * ws9.n_sct
for sec in arange(ws9.n_sct) + 1: # from section 1 to "next apex - 1"
_, maps_sect, _ = fda.fitData( ws9.d1d_l_dt[:, 0::ws9.nps], ws9.d1d_l_dt[:, sec::ws9.nps],
sections=[0,], nps=1, rcond=1e-7, nrep=50)
ws9.fmodels_l.append(fda.meanMat(maps_sect))
sys.stdout.write('.')
print " done"
# compute indices which select only the CoM state: [com_z, v_y, v_z, v_x] ("height, horizontal speed, vert. speed, lat. speed")
ls = len(ws9.k9.selection)
ws9.FM_CoM_sel_idx = array([0, ls-1, ls, ls-2])
step 9: compare trajectories SLIP vs. Floquet models
to content
In [4]:
# test prediction
rvar = []
for nr, fm in enumerate(ws9.fmodels_r[:-1]):
pred = dot(fm, ws9.d1d_r_dt[:, 0::ws9.nps].T).T #all_kin_r.T).T
meas = ws9.d1d_r_dt[:, nr+1:: ws9.nps]
rvar.append(var(meas - pred, axis=0) / var(meas, axis=0))
figure(figsize(16,8))
phase_ticks = linspace(1./len(ws9.fmodels_r), 1, len(ws9.fmodels_r))
pcolor(arange(len(rvar[0])+1), phase_ticks, vstack(rvar))
xlabel('coordinate #')
ylabel('phase to predict (in $\pi$)')
gca().set_xticks(arange(len(ws9.k.kin_labels)+1) + .5)
gca().set_xticklabels(ws9.k.kin_labels, rotation=90)
colorbar()
title('relative remaining variance after prediction (single step!!)')
clim(0,1)
step 9: compare trajectories SLIP vs. Floquet models
to content
step 9: compare trajectories SLIP vs. Floquet models
to content
Idea:
In [5]:
def reord_idx(labels):
"""
returns indices for re-ordering dataset data such that first three columns are CoM state (SLIP state)
:args:
labels [list]: a list of labels that describe the ordering of the columns. it is scanned for
com_z (-> idx0) , v_com_x (-> idx2), v_com_y (idx1)
:returns:
idcs (array): an array of the size of labels that starts with [idx0, idx1, idx2] and
contains all numbers from 0 to len(labels)-1.
This can be used for indexing as follows:
re_ordered_data = original_data[:, reord_idx(kin_labels)]
"""
idx0 = [idx for idx, val in enumerate(labels) if val.lower() == 'com_z'][0]
idx1 = [idx for idx, val in enumerate(labels) if val.lower() == 'v_com_y'][0]
idx2 = [idx for idx, val in enumerate(labels) if val.lower() == 'v_com_x'][0]
ridx = list(set(range(len(labels))) - set([idx0, idx1, idx2]))
idcs = hstack([idx0, idx1, idx2, ridx])
return idcs
#re-order: CoM at first
states_r = ws9.dataset_r.all_kin_r[:, reord_idx(ws9.dataset_r.kin_labels)]
states_l = ws9.dataset_l.all_kin_l[:, reord_idx(ws9.dataset_l.kin_labels)]
# note: here, implicitly a periodic orbit is computed (given in the reference states)
cmaps, nmaps, refs = getControlMaps(states_r, states_l, ws9.dataset_r)
ws9.ref_state_r, ws9.ref_state_l, ws9.ref_param_r, ws9.ref_param_l, ws9.ref_addDict = refs
ws9.ctrl_r, ws9.ctrl_l = cmaps
ws9.nmap_r, ws9.nmap_l = nmaps
ws9.ref_state_r = hstack([ws9.ref_state_r, (len(ws9.dataset_r.kin_labels) - 3) * [0, ]])
ws9.ref_state_l = hstack([ws9.ref_state_l, (len(ws9.dataset_l.kin_labels) - 3) * [0, ]])
# create autonomous system and check periodicity
cSLIP = get_auto_sys(ws9.ref_param_r, ws9.ref_param_l, ws9.ref_state_r, ws9.ref_state_l, ws9.ctrl_r, ws9.ctrl_l, ws9.nmap_r, ws9.nmap_l, ws9.ref_addDict)
print "periodicity test:"
print cSLIP(ws9.ref_state_r) - ws9.ref_state_r
In [5]:
In [6]:
# requirement: dist_state
dist_state = ws9.ref_state_r.copy() * 1.1
cSLIP_f = get_auto_sys(ws9.ref_param_r, ws9.ref_param_l, ws9.ref_state_r, ws9.ref_state_l,
ws9.ctrl_r, ws9.ctrl_l, ws9.nmap_r, ws9.nmap_l, ws9.ref_addDict, full_info=True)
# define how many step to run until convergence is reached
nstride = 10
fs_f, (sim_t_ref, sim_states_ref) = cSLIP_f(ws9.ref_state_r)
sim_t_d = []
sim_s_d = []
fs_d = dist_state
for rep in range(nstride):
fs_d, (sim_t, sim_states) = cSLIP_f(fs_d)
oldtime = 0
if len(sim_t_d) > 0:
oldtime = sim_t_d[-1][-1]
sim_t_d.append(sim_t[:-1] + oldtime)
sim_s_d.append(sim_states[:-1,:])
sim_s_d.append(sim_states[-1, :][newaxis, :])
sim_t_d.append(sim_t[-1] + oldtime)
sim_s_d = vstack(sim_s_d)
sim_t_d = hstack(sim_t_d)
# now, compute phase (after simulation is completed)
# at the end, phase is 0 (or 2pi)
# total amount of phase elaped is: (time / sim_t_ref) * 2pi
phi_total = sim_t_d[-1] / sim_t_ref[-1] * (2*pi)
# final phase is: nstride * 2pi
final_phase = nstride * 2*pi
initial_phase = final_phase - phi_total
# disturbed phase is no:
sim_phi_d = sim_t_d /sim_t_ref[-1] * 2*pi + initial_phase
print "phi0:", initial_phase
def getPhase(state, system, nstride=10):
"""
computes the phase of the given SLIP system at the state "state".
:args:
state [n-by-1 array]: the initial state of the system
system [autonomous SLIP]: the dynamical system (SLIP model), obtained e.g. by models.slip.get_auto_sys
*NOTE* the system must be stable!
nstride [int]: number of strides to be simulated before convergence is assumed
:returns:
(phi0, T) [float, float]: the phase correspoding to the initial condition and the average cycle duration
"""
# run #nstride strides
sim_t_d = []
sim_s_d = []
fs_d = array(state).copy()
for rep in range(nstride):
fs_d, (sim_t, sim_states) = cSLIP_f(fs_d)
oldtime = 0
if len(sim_t_d) > 0:
oldtime = sim_t_d[-1][-1]
sim_t_d.append(sim_t[:-1] + oldtime)
sim_s_d.append(sim_states[:-1,:])
sim_s_d.append(sim_states[-1, :][newaxis, :])
sim_t_d.append(sim_t[-1] + oldtime)
sim_s_d = vstack(sim_s_d)
sim_t_d = hstack(sim_t_d)
# assume that solution is now periodic -> simulate "reference" stride
fs_f, (sim_t_ref, sim_states_ref) = system(fs_d)
# now, compute phase (after simulation is completed)
# at the end, phase is 0 (or 2pi)
# total amount of phase elaped is: (time / sim_t_ref) * 2pi
phi_total = sim_t_d[-1] / sim_t_ref[-1] * (2*pi)
# final phase is: nstride * 2pi
final_phase = nstride * 2*pi
initial_phase = final_phase - phi_total
# disturbed phase is no:
return initial_phase, sim_t_ref[-1]
print "from function:", getPhase(dist_state, cSLIP_f)
In [7]:
# create system
cSLIP_f = get_auto_sys(ws9.ref_param_r, ws9.ref_param_l, ws9.ref_state_r, ws9.ref_state_l, ws9.ctrl_r,
ws9.ctrl_l, ws9.nmap_r, ws9.nmap_l, ws9.ref_addDict, full_info=True)
# run reference motion
fs_f, (sim_t, sim_states) = cSLIP_f(ws9.ref_state_r)
figure(figsize=(8,6))
plot(sim_t, sim_states[:,1],'r')
plot(sim_t, sim_states[:,3],'r--')
# run disturbed motion
dist_state_r = ws9.ref_state_r.copy()
dist_state_r[0] += .05 # increase apex height by 5cm
dist_state_r[1] += .15 # increase velocity by .15 m/s
fs_f, (sim_t_d, sim_states_d) = cSLIP_f(dist_state_r)
plot(sim_t_d, sim_states_d[:,1],'g')
plot(sim_t_d, sim_states_d[:,3],'g--')
Out[7]:
step 9: compare trajectories SLIP vs. Floquet models
to content
A figure that displays the predicted CoM and measured CoM as function of phase.
"Predict from phase 0 to pi"
for both sides, left and right, do:
ws9.phi_apex_r, ws9.s_phi_apex_l [OK -> 9.1]create Phaser for uncontrolled SLIP
for each apex do:
In [8]:
# fixme: change this to the fixed phases! These are: [0 ... 2pi) / nps
fixed_phases = linspace(0, 2*pi, ws9.nps, endpoint=False)
pred_phases = roll(fixed_phases, -1)
pred_phases[-1] = 2*pi # change 0 -> 2pi
phi_out_r = pred_phases[:25] + ws9.phi_apex_r
phi_out_l = pred_phases[:25] + ws9.phi_apex_l
#fixed_phases = linspace(0, 2*pi, ws9.nps, endpoint=False)
In [9]:
# find first index after
step = 1100
phi_apex = mod(ws9.a_phi_apex_r[step], 2*pi)
#idx_ar = argmin(abs(fixed_phases - ws9.phi_apex_r))
#if fixed_phases[idx_ar] - ws9.phi_apex_r <= 0:
# idx_ar += 1
#idx_al = argmin(abs(fixed_phases - ws9.phi_apex_l))
#if fixed_phases[idx_al] - ws9.phi_apex_l <= 0:
# idx_al += 1
#
#phi_firstAfter_r = fixed_phases[idx_ar]
#phi_firstAfter_l = fixed_phases[idx_al]
# compute mappings from phase state to "first state after apex"
kins = ws9.k9.get_kin()
phases = [x['phi2'] for x in ws9.k9.raw_dat]
im_r = []
if True:
lidat = []
lodat = []
for kin, phi, iphi in zip(kins, phases, ws9.phases_d1d_r):
phi_start = iphi[1]
phi_end = iphi[-ws9.nps + 1] + 2.*pi
#phi_end = iphi[-1]
phi_odat = linspace(phi_start, phi_end, len(iphi) / ws9.nps , endpoint=False)
phi_idat = phi_odat - (phi_start - phi_apex) # fixed_phases[idx_ar] + ws9.phi_apex_r
# interpolate data according to phi_req (exclude position in horizontal plane)
idat_raw = vstack([interp(phi_idat, phi.squeeze(), kin[row, :]) for row in range(2, kin.shape[0])])
lidat.append(idat_raw)
odat_raw = vstack([interp(phi_odat, phi.squeeze(), kin[row, :]) for row in range(2, kin.shape[0])])
lodat.append(odat_raw)
#odat.append(vstack([fda.dt_movingavg(interp(phi_odat, phi.squeeze(), kin[row,:]), 30) for row in range(2, kin.shape[0])]))
#something like odat = interp(phi_odat, phi_raw, kin_coord)
idat = fda.dt_movingavg(hstack(lidat).T, 30)
odat = fda.dt_movingavg(hstack(lodat).T, 30)
print "bootstrapping..."
print "=" * 50
for rep in range(50):
sel_idx = randint(0, idat.shape[0], idat.shape[0])
init_mapping_r = dot(pinv(idat[sel_idx, :], rcond=1e-8), odat[sel_idx, :]).T
im_r.append(init_mapping_r)
sys.stdout.write('.')
print "done"
im_r_m = fda.meanMat(im_r)
lidat = []
lodat = []
for kin, phi, iphi in zip(kins, phases, ws9.phases_d1d_r):
phi_start = iphi[1]
phi_end = iphi[-ws9.nps + 1] + 2.*pi
#phi_end = iphi[-1]
phi_odat = linspace(phi_start, phi_end, len(iphi) / ws9.nps , endpoint=False)
phi_idat = phi_odat - (phi_start - phi_apex) # fixed_phases[idx_ar] + ws9.phi_apex_r
# interpolate data according to phi_req (exclude position in horizontal plane)
idat_raw = vstack([interp(phi_idat, phi.squeeze(), kin[row, :]) for row in range(2, kin.shape[0])])
lidat.append(idat_raw)
odat_raw = vstack([interp(phi_odat, phi.squeeze(), kin[row, :]) for row in range(2, kin.shape[0])])
lodat.append(odat_raw)
#odat.append(vstack([fda.dt_movingavg(interp(phi_odat, phi.squeeze(), kin[row,:]), 30) for row in range(2, kin.shape[0])]))
#something like odat = interp(phi_odat, phi_raw, kin_coord)
idat = fda.dt_movingavg(hstack(lidat).T, 30)
odat = fda.dt_movingavg(hstack(lodat).T, 30)
init_mapping_r = dot(pinv(idat, rcond=1e-8), odat).T
# verify that map is close to 1 !!! (-> mostly true for positions; not so much for velocities!)
figure(figsize=(12,6))
subplot(1,2,1)
pcolor(init_mapping_r[::-1,:]), clim(-1,1)
title('map from all data')
subplot(1,2,2)
pcolor(im_r_m[::-1, :]), clim(-1,1)
title('avg. from bootstrapping')
Out[9]:
In [10]:
dat = ws9.d1d_r_dt
idx = 2
m1 = dot(pinv(dat[:, idx::50]), dat[:, idx+1::50])
figure()
pcolor(m1), clim(-1,1)
colorbar()
Out[10]:
In [11]:
# sanity check
figure(), plot(idat_raw[0,:])
print idat_raw.shape
plot(ws9.d1d_r[-326:, 0],'rd')
title('sanity check: comparison of different views of the same data')
#ws9.d1d_r.shape
Out[11]:
-space left-
In [12]:
# compute baseline
# step = 1203 has to be defined somewhere else
baseline_r = roll(vstack([ws9.d1d_r_trend[step,x*ws9.nps:(x+1)*ws9.nps] for x in ws9.FM_CoM_sel_idx]), -1, axis=1)
baseline_l = roll(vstack([ws9.d1d_l_trend[step,x*ws9.nps:(x+1)*ws9.nps] for x in ws9.FM_CoM_sel_idx]), -1, axis=1)
print "NOTE: baseline data is already 'rolled' - first index [0] corresponds to first section to predict!"
# add belt speed
vBelt = mean([d.vBelt for d in ws9.SlipData])
baseline_r[1, :] += vBelt
baseline_l[1, :] += vBelt
fmdl_preds = []
apexstate = idat[step, :]
# compute predictions
for mdl in ws9.fmodels_r:
scmdl = dot(mdl, init_mapping_r) # map to 1st section -> map to requested section
scmdl = dot(mdl, im_r_m) # map to 1st section -> map to requested section
#scmdl = mdl # omit initial mapping from apex state -> section after apex
pred = dot(scmdl, apexstate)
fmdl_preds.append(pred)
fmdl_pred_r = vstack(fmdl_preds)
com_pred_r_dt = fmdl_pred_r[:, ws9.FM_CoM_sel_idx]
com_pred_r = com_pred_r_dt + baseline_r[:, :com_pred_r_dt.shape[0]].T
# include SLIP prediction
# SLIP_IC = apexstate[
In [13]:
if not hasattr(ws9, 'ks'):
ws9.ks = mio.KinData()
ws9.ks.load(ws9.subject, ws9.ttype) # subject in [1,2,3,7], ttype=1
ws9.ks.selection = ['com_x', 'com_y', 'com_z', 'r_anl_y - com_y', 'r_anl_z - com_z', 'r_anl_x - com_x']
k_apex = ws9.ks.get_kin_apex(phases = ws9.k.all_phases_r)
aka = hstack(k_apex).T[:, 2:]
kin_labels = ws9.ks.selection[2:] + ['v_' + x for x in ws9.ks.selection]
print "aka.shape:", aka.shape
print kin_labels
aka_s = aka[:, reord_idx(kin_labels)] # apex states from "kin at apex" (quadratic interpolation) INCLUDING vertical velocity!
# find index of v_com_z
for idx, lbl_idx in enumerate(reord_idx(kin_labels)):
if kin_labels[lbl_idx] == 'v_com_z':
break
print "identified index:", lbl_idx
aka_sx = hstack([aka_s[:, :lbl_idx], aka_s[:, lbl_idx + 1:]])
# detrend non-CoM states!
aka_sx[:, 3:] = fda.dt_movingavg(aka_sx[:, 3:], 30)
In [14]:
print [(rep, kin_labels[val]) for rep, val in enumerate(reord_idx(kin_labels))]
print "v_com_z?"
print aka_sx[step, : ]
print aka_s[step, : ]
In [15]:
print "reference state:", ws9.ref_state_r
CoM_exp = aka_sx[step, :].copy()# + baseline_r[:,0]
print "-------------- Comparison of 'apex state' with regularly sampled data [y, vx, vz]: ------- "
print ' '.join([str(elem) for elem in CoM_exp[:3]])
print ws9.d1d_r[step, 0], ws9.d1d_r[step, 50*23], ws9.d1d_r[step, 50*22]
print "-------------------------------------------------------------------------------------------"
CoM_exp[1] += vBelt
#CoM_exp = aka[1203, ws9.FM_CoM_sel_idx[array([0,1,3])]]# + baseline_r[:,0]
#CoM_exp[1] += vBelt
#print CoM_exp
SLIP_IC = CoM_exp
print "IC:", SLIP_IC
cSLIP_r = get_auto_sys(ws9.ref_param_r, ws9.ref_param_l, ws9.ref_state_r, ws9.ref_state_l,
ws9.ctrl_r, ws9.ctrl_l, ws9.nmap_r, ws9.nmap_l, ws9.ref_addDict, full_info=True)
fs, (sim_t, sim_state) = cSLIP_r(SLIP_IC)
In [16]:
phi0, T_sim = getPhase(SLIP_IC, cSLIP_r)
print sim_state.shape, sim_t.shape
In [17]:
_, (ref_sim_t, ref_sim_state) = cSLIP_r(ws9.ref_state_r)
ref_sim_phase = ref_sim_t / ref_sim_t[-1] * 2*pi
#fs, (sim_t, sim_state) = cSLIP_r(SLIP_IC)
phi0, T_sim = getPhase(SLIP_IC, cSLIP_r)
phi_mdl = sim_t / T_sim * 2*pi + phi0
# adapt t_sim_50 such that these points have the same phase as the experimental data
t_sim_50 = linspace(0, sim_t[-1], 50, endpoint=False)
phi_50 = (arange(25) + 1) * .02 * 2* pi
vx_sim = interp(phi_50, phi_mdl, sim_state[:, 3])
vx_sim_ref = interp(phi_50, ref_sim_phase, ref_sim_state[:, 3])
y_sim = interp(phi_50, phi_mdl, sim_state[:, 1])
y_sim_ref = interp(phi_50, ref_sim_phase, ref_sim_state[:, 1])
vz_sim = interp(phi_50, phi_mdl, sim_state[:, 5])
vz_sim_ref = interp(phi_50, ref_sim_phase, ref_sim_state[:, 5])
#plot(vx_sim[1:], 'm', label='controlled SLIP') # old version
#y_sim = interp(t_sim_50, sim_t, sim_state[:, 1])
figure(19,figsize=(12,10))
clf()
subplot(3,1,1) # vx
plot(baseline_r[1, :],'r', label='baseline (average motion)')
plot(vx_sim_ref,'r--', label='SLIP periodic orbit')
plot(com_pred_r_dt[:,1] + 3,'b', label='deviation from baseline (Floquet model)')
plot([0, 24], [3, 3],'b--')
plot(com_pred_r[:,1],'k--', label='predicted (Floquet model + baseline)')
vx_slice = slice(ws9.FM_CoM_sel_idx[1] * ws9.nps , (ws9.FM_CoM_sel_idx[1] + 1) * ws9.nps)
vz_slice = slice(ws9.FM_CoM_sel_idx[3] * ws9.nps , (ws9.FM_CoM_sel_idx[3] + 1) * ws9.nps)
plot(0, apexstate[ws9.FM_CoM_sel_idx[1]] + baseline_r[1, 0], 'gd', label='apex state (Floquet model IC)')
ylim(2.5, 3.1)
plot(vx_sim[0:], 'm')
vx_exp = ws9.d1d_r[step, vx_slice] + vBelt
y_exp = ws9.d1d_r[step, :50]
vz_exp = ws9.d1d_r[step, vz_slice]
#print "NOTE - vx_exp is *not* yet rolled by 1 frame!"
plot(vx_exp[1:],'k', label='experimental data')
print "belt speed is ", vBelt
legend()
title('horizontal speed')
subplot(3,1,2) # y
plot(y_sim[0:], 'm', label='controlled SLIP')
plot(y_sim_ref,'r--', label='SLIP periodic orbit')
#plot(y_sim[1:], 'm')
plot(y_exp[1:],'k', label='experimental data')
plot(com_pred_r[:,0],'k--', label='predicted (Floquet model + baseline)')
plot(baseline_r[0,:],'r', label='baseline')
title('vertical position')
subplot(3,1,3) #vz
plot(vz_sim[0:], 'm', label='controlled SLIP')
plot(vz_sim_ref,'r--', label='SLIP periodic orbit')
#plot(vz_sim[1:], 'm')
plot(vz_exp[1:],'k', label='experimental data')
plot(com_pred_r[:,3],'k--', label='predicted (Floquet model + baseline)')
plot(baseline_r[3,:],'r', label='baseline')
title('lateral speed')
Out[17]:
In [18]:
# sanity check
print "selected step:", step
figure()
vx_slice = slice(ws9.FM_CoM_sel_idx[1] * ws9.nps , (ws9.FM_CoM_sel_idx[1] + 1) * ws9.nps)
plot(hstack([ws9.d1d_r[x, vx_slice] for x in range(1200, 1207)]), 'b', label='experimental data')
xval = 50 * arange(7)
delta=0
plot(hstack([ws9.d1d_r_dt[x, vx_slice] + ws9.d1d_r_trend[x, vx_slice] for x in range(1200, 1207)]), 'r--', label='exp: trend + detrend')
plot(xval, idat[range(1200 + delta,1207 + delta), ws9.FM_CoM_sel_idx[1]],'rd', label='ICs for floquet model')
plot(hstack([ws9.d1d_r_trend[x, vx_slice] for x in range(1200, 1207)]), 'k--', label='exp: baseline')
plot(hstack([ws9.d1d_r_dt[x, vx_slice] for x in range(1200, 1207)]), 'g--', label='exp: deviation from baseline')
legend()
Out[18]:
In [19]:
# create controlled "left" and "right" SLIP (model that starts with left and right step)
cSLIP_r = get_auto_sys(ws9.ref_param_r, ws9.ref_param_l, ws9.ref_state_r, ws9.ref_state_l,
ws9.ctrl_r, ws9.ctrl_l, ws9.nmap_r, ws9.nmap_l, ws9.ref_addDict, full_info=True)
cSLIP_l = get_auto_sys(ws9.ref_param_l, ws9.ref_param_r, ws9.ref_state_l, ws9.ref_state_r,
ws9.ctrl_l, ws9.ctrl_r, ws9.nmap_l, ws9.nmap_r, ws9.ref_addDict, full_info=True)
In [19]:
In [20]:
# get phase of apex
phi_r = mod(ws9.a_phi_apex_r[step], 2*pi)
In [21]:
# compute predictions
ws9.fmodels_r[0].shape
Out[21]:
In [22]:
figure()
plot(baseline_r[2, :],'b.-')
plot(baseline_l[2, :],'b--')
plot([24,24],[-.5, .5],'r--')
plot([0,50],[0,0],'k--')
Out[22]:
In [23]:
x = arange(12).reshape(3,4)
In [24]:
print x
print roll(x, 1, axis=1)
In [25]:
ws9.display()
In [26]:
k = mio.KinData()
k.load(2, 1) # subject in [1,2,3,7], ttype=1
#SlipData = [mi.Struct(mio.mload('../data/2013-newSlip/SLIP_s%it%ir%i.dict' % (ws9.subject, ws9.ttype, rep)))
# for rep in k9.reps]
k.selection = ['com_x', 'com_y', 'com_z', 'r_anl_y - com_y', 'r_anl_z - com_z', 'r_anl_x - com_x', 'l_anl_y - com_y', 'l_anl_z - com_z', 'l_anl_x - com_x',]
d1d = k.make_1D(nps=50)[:, 100:]
In [27]:
d1d -= mean(d1d, axis=0)
u,s,v = svd(d1d.T, full_matrices = False)
print v.shape
In [28]:
figure()
for rep in range(4):
subplot(4,1,1 + rep)
plot(v[rep,:],'.')
plot([0,1940],[0,0],'k--')
ylabel('PC ' + str(rep + 1))
if rep==0:
title('scores on first principal components on CoM + Ankles-System')
In [29]:
SlipData = [mi.Struct(mio.mload('../data/2013-newSlip/SLIP_s%it%ir%i.dict' % (2, 1, rep)))
for rep in k.reps]
d9 = build_dataset(k, SlipData)
pr = d9.all_param_r.copy()
pr -= mean(pr, axis=0)
pr /= std(pr, axis=0)
u,s,v = svd(pr.T, full_matrices=False)
In [30]:
figure()
for rep in range(5):
subplot(5,1,1 + rep)
plot(v[rep,:],'r.')
plot([0,1940],[0,0],'k--')
ylabel('PC ' + str(rep + 1))
if rep==0:
title('scores on first principal components on SLIP parameters')
print "s**2 normalized:", s**2 / sum(s**2)
In [31]:
ws9.ref_state_r
dist_state = ws9.ref_state_r.copy()
dist_state[2] += .5 # lateral velocity
fs = dist_state.copy()
all_simt, all_sims = [], []
lasttime = 0
lastpos = [0, 0, 0]
for rep in range(10):
fs, (sim_t, sim_states) = cSLIP_r(fs)
all_simt.append(sim_t + lasttime)
sim_states[:, :3] += lastpos
all_sims.append(sim_states)
lasttime = sim_t[-1]
lastpos = sim_states[-1,:3]
sim_states = vstack(all_sims)
sim_t = hstack(all_simt) #
In [133]:
Out[133]:
In [32]:
figure()
plot(sim_states[:,0], sim_states[:,2],'r.')
Out[32]:
In [119]:
eig_nps = 4
k3s = ws9.k9.make_1D(nps=eig_nps)
k3s = fda.dt_movingavg(k3s[:, eig_nps * 2:], 30)
k3s.shape
Out[119]:
In [112]:
138 / 3
Out[112]:
In [ ]:
### quickly compare EV's of multi-section map with J of controlled SLIP
In [141]:
cr = lambda x: cSLIP_r(x)[0]
J = mi.calcJacobian(cr, ws9.ref_state_r)
In [120]:
_, m1, idcs = fda.fitData(k3s[:, ::eig_nps], k3s[:, 1::eig_nps], nps=1, nrep=100, rcond=1e-6)
_, m2, idcs = fda.fitData(k3s[:, 1::eig_nps], k3s[:, 2::eig_nps], nps=1, nrep=100, rcond=1e-6)
_, m3, idcs = fda.fitData(k3s[:, 2::eig_nps], k3s[:, 3::eig_nps], nps=1, nrep=100, rcond=1e-6)
_, m4, idcs = fda.fitData(k3s[:-1, 3::eig_nps], k3s[1:, 0::eig_nps], nps=1, nrep=100, rcond=1e-6)
In [123]:
m = [reduce(dot, [x1.T, x2.T, x3.T, x4.T]) for x1,x2,x3, x4 in zip(m1, m2, m3, m4)]
In [147]:
import libshai.util as ut
vi = ut.VisEig(127, False)
for A in m:
vi.add(eig(A)[0])
figure(figsize=(8,7))
vi.vis1()
xlim(-1,1)
ylim(-1,1)
xlabel('real part')
ylabel('imaginary part')
#ncnt = 0
#for ev in eig(J)[0]:
# lbl = None if ncnt > 0 else "Eigenvalues of controlled ankle-SLIP"
# plot(ev.real, ev.imag, 'rd', label=lbl)
# ncnt += 1
title('eigenvalue distribution of return maps\nsubject 2')
#legend()
Out[147]:
In [130]:
print ws9.k9.subject
print ws9.subject
In [143]:
figure()
for ev in eig(J)[0]:
plot(ev.real, ev.imag, 'rd')
ylim(-1,1)
xlim(-1,1)
axis('equal')
Out[143]:
In [ ]:
get_auto_sys(ws9.ref_param_r, ws9.ref_param_l, ws9.ref_state_r, ws9.ref_state_l,
ws9.ctrl_r, ws9.ctrl_l, ws9.nmap_r, ws9.nmap_l, ws9.ref_addDict, full_info=True)
fs, (sim_t, sim_state) = cSLIP_r(SLIP_IC)